import socket
import struct
import sys
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import rclpy.qos
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist
from geometry_msgs.msg import PoseStamped, TransformStamped
from interfaces_consensus.msg import CommandMsg
from builtin_interfaces.msg import Time
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Point, Quaternion
from rclpy.parameter import Parameter
import time
import threading

class GoalNode(Node):
    def __init__(self):
    # 定义本地接收地址和端口

        super().__init__('sub_uav_pos')

        self.virtual_uav_publishers = {}
        self.uav_id=int(sys.argv[1])
        # self.uav_id=1

        
        # # UDP点对点发送
        # self.server_address = (str(sys.argv[2]), 12345)
        # # self.server_address = (''127.0.0.1'', 12345)
        # self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #创建套接字
        # self.sock.bind(self.server_address) # 绑定套接字到指定地址和端口

        # UDP多播接收
        MCAST_GRP = '224.0.0.1'
        MCAST_PORT = 2025
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
        self.sock.bind(('', MCAST_PORT))
        mreq = struct.pack("=4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY)
        # self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
        # 尝试加入多播组，直到成功
        while True:
            try:
                self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
                time.sleep(1)
                print(f"成功加入多播组 {MCAST_GRP} 并绑定端口 {MCAST_PORT}")
                break  # 成功后跳出循环
            except OSError as e:
                print(f"错误: {e}. 尝试重新连接...")
                time.sleep(1)  # 等待 1 秒后重试
        # self.sock.setblocking(False)  #设置为非阻塞模式
        # self.sock.settimeout(1.0)  #设置超时

    

        #定义定时器运行主体程序
        # self.create_timer(0.001,self.run)

        # #启动接收数据的线程
        # self.thread = threading.Thread(target=self.run)
        # self.thread.daemon = True
        # self.thread.start()

        

        print("Waiting to receive message...")
        self.run()

    def virtual_uav_pub(self, uavId, msg):
        topic_name = f"drone{uavId}/velocity_and_position"
        if topic_name in self.virtual_uav_publishers:
            publisher = self.virtual_uav_publishers[topic_name]
            # print(f"topic: {topic_name} already existed!")
        else:
            publisher = self.create_publisher(Float64MultiArray, topic_name, 10)
            self.virtual_uav_publishers[topic_name] = publisher
            # print(f"topic: {topic_name} publish!")
        publisher.publish(msg)

    def find_element_with_id(self, arr):
        # 遍历二维数组中的每个子数组
        for subarray in arr:
            # 如果子数组中包含 target_id，返回这个子数组
            if self.uav_id in subarray:
                return subarray
        # 如果没有找到，返回 None
        return None

    def run(self):

        # 解包数据
        # 'ii64s' -> 两个整数和一个最多64字符的字符串
        # '????????' -> 8个布尔值
        while rclpy.ok():
            try:
                
                # 接收数据
                data, address = self.sock.recvfrom(4096)
                # print(data)
                # print(len(data))
                # 解包数据，'d' 表示 double（8字节）
                unpacked_data = struct.unpack("2c", data[:2])
                type = unpacked_data[0]
                head = unpacked_data[1]
                # print(f"type={type}--head={head}")

                #0 导调
                #1 集群状态
                #2 分群
                #3 障碍物位姿
                #4 无人机位姿

                #5 分群结果指令
                #6 控制指令包括目标点、队形、除分群外的其他指令
                
                if type == b'\x01' and head == b'\x14':
                    unpacked_data = struct.unpack('=i6d', data[2:54])
                    uav_id = int(unpacked_data[0])
                    longitude = unpacked_data[1] 
                    latitude = unpacked_data[2]
                    altitude = unpacked_data[3]
                    x_vel = unpacked_data[4]
                    y_vel = unpacked_data[5]
                    z_vel = unpacked_data[6]
                    if uav_id !=self.uav_id:
                        msg = Float64MultiArray()
                        msg.data = [8.8,longitude,latitude,altitude,x_vel,y_vel,z_vel]
                        self.virtual_uav_pub(uav_id,msg)
                        self.get_logger().info(f"pub uav{uav_id}pos: {msg}")

                # 发布消息                
            except Exception as e:
                self.get_logger().error(f"{e}\ndata: {data}")
            




def main(args=None):
    rclpy.init(args=args)
    node = GoalNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.sock.close()
        node.destroy_node()   
        rclpy.shutdown() 
    
if __name__ == "__main__":
    main()    
